#!/usr/bin/python3
import rospy
from drone_pkg.msg import SendString
import os
import time
from anorosdt.msg import AnoRosDT_User
import subprocess
import signal
from drone_pkg.srv import *



def start_process(cmd):
    terminal_process = subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', f'{cmd}; exec bash'])
    return terminal_process


def Laser_Init():
        os.system("gpio mode 10 out")
        os.system("gpio mode 13 out")
        os.system("gpio write 10 1")
        os.system("gpio write 13 1")

def Laser_Control(laser_id,state):
    if laser_id == 1 and state == 0:
        os.system("gpio write 10 1")
    if laser_id == 1 and state == 1:
        os.system("gpio write 10 0")
    if laser_id == 0 and state == 0:
        os.system("gpio write 13 1")
    if laser_id == 0 and state == 1:
        os.system("gpio write 13 0")

def Laser_ON(laser_id, t):
     Laser_Control(laser_id, 1)
     time.sleep(t * 0.1)
     Laser_Control(laser_id, 0)

def shutdown():
     Laser_Control(1, 0)
     Laser_Control(0, 0)
     

cartographer_state = 0
camera_state = 0
control_state = 0
anoros_state = 0
time_state = 0
#控制激光,当control， camera运行后不再改变激光灯
laser_flag = 0



def callback(s):
    global cartographer_state, camera_state, control_state, laser_flag,anoros_state,time_state
    global client
    a = s.s161
    # print(a)
    # if (a > 0 and a < 100  and laser_flag == 0):
    #       Laser_Control(0, 1)
    #       Laser_Control(1, 1)
    # elif ((a == 0 or a >= 100) and laser_flag == 0):
    #     Laser_Control(0, 0)
    #     Laser_Control(1, 0)

    if a == 0:
         cartographer_state = 0
         camera_state = 0
         control_state = 0
         time_state = 0
    #刷新雷达
    if a == 101:
        if cartographer_state == 0:
            #start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/cartographer.sh')
            #start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/jixun10.sh')
            start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/jixun41.sh')
            cartographer_state = 1
    #刷新摄像头
    if a == 102:
        if camera_state == 0:
             #start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/QRcodeDetect.sh')
             #start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/jixun11.sh')
             start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/jixun42.sh')
             camera_state = 1
             #运行摄像头后不使用激光
             laser_flag = 1
    #刷新控制
    if a == 103:
        if control_state == 0:
            start_process('/home/orangepi/catkin_ws/src/drone_pkg/scripts/StartControl.sh')
            control_state = 1
            #运行control后不使用激光
            laser_flag = 1
    
    #开始计时
    if a == 100:
         if time_state == 0:
              time_state = 1
              client.wait_for_service()
              time_state = 0
              req.state1 = 1
              client.call(req)
              rospy.loginfo("start time")


    
    
         
         
        


rospy.init_node("Laser_Control", anonymous=True)
rate = rospy.Rate(10)
sub_laser = rospy.Subscriber('/Orangepi_Control', AnoRosDT_User, callback, queue_size=10)
client = rospy.ServiceProxy("Start_Time", Start_time)
req = Start_timeRequest()
rospy.on_shutdown(shutdown)
Laser_Init()
while not rospy.is_shutdown():
    rate.sleep()



